package org.usfirst.team3132.demo.libs;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.PIDController;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Victor;

/**
 *
 * @author James Ridey
 */
public class MotorDrive
{
    float kP = 0.1F;
    float kI = 0F;
    float kD = 0F;
    
    //Encoder int values
    double pidGradient;
    double currentGet;
    double oldGet;
    double currentTime;
    double oldTime;
    
    int smoothingfactor;
    boolean init;
    
    //Use Jaguars now because of PIDController (Victors are for Mate)
    Victor motor;
    Joystick joystick;
    Encoder encoder;
    
    PIDController pidController;
    EncoderPID pidEncoderSource;
    
    public MotorDrive(int joystickport, int motorport, int sensorportA, int sensorportB, int sensorslot, int maxspeed, int smoothingfactor)
    {
        joystick = new Joystick(joystickport);
        motor = new Victor(motorport);
        encoder = new Encoder(sensorslot,sensorportA,sensorslot,sensorportB);
        pidEncoderSource = new EncoderPID();
        this.smoothingfactor = smoothingfactor;
    }
    
    public void init()
    {
        pidGradient = 0;
        currentGet = 0;
        oldGet = 0;
        currentTime = 0;
        oldTime = 0;

        encoder.setPIDSourceParameter(Encoder.PIDSourceParameter.kRate);
        motor.setSafetyEnabled(false);
        
        pidController = new PIDController(kP,kI,kD,pidEncoderSource,motor);
        pidController.setInputRange(-2000F, 2000F); //Not sure
        pidController.setOutputRange(-1F, 1F);
        
        init = false;
    }
    
    public void loop()
    {
        if (!init)
        {   
            init = true;
            encoder.reset();
            encoder.start();

            pidController.setContinuous(false); 
            pidController.reset();
            pidController.enable();

            oldTime = Timer.getFPGATimestamp();
            oldGet = encoder.get();
        }
        
        //System.out.println(joystick.getY());
        double setpoint = pow(joystick.getY(), smoothingfactor);
        System.out.println(setpoint + " Setpoint");
        pidController.setSetpoint(setpoint * 2000);
        //System.out.println(pidController.get());
        
        currentTime = Timer.getFPGATimestamp();
        currentGet = encoder.get();
        
        // calculate and display rate
        pidGradient = (currentGet - oldGet) / (currentTime - oldTime);
        pidEncoderSource.addValue(pidGradient);

        System.out.println(pidEncoderSource.pidGet());
        oldTime = currentTime;
        oldGet = currentGet;
    }

    public void die()
    {
        pidEncoderSource.reset();
        encoder.stop();
        pidController.disable();
    }
    
    public double pow(double original, double power)
    {
        double output = 1;
        for (int i = 0;i < power; i++)
        {
            output *= Math.abs(original);
        }
        if (original < 0) output *= -1;
        return output;
    }
    
    public void changePid(float kP, float kI, float kD)
    {
        this.kP = kP;
        this.kI = kI;
        this.kD = kD;
    }
}
